﻿using System;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Threading;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Media.Imaging;

using Coding4Fun.Kinect.Wpf;

using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit;

using Phidgets;

namespace Coding4Fun.KinectTurret
{
	/// <summary>
	/// Interaction logic for MainWindow.xaml
	/// </summary>
	public partial class MainWindow : Window
	{
		private const int MaxSkeletons = 6; 
		
		AdvancedServo _servoController;
		InterfaceKit _relayController;
		AdvancedServoServo _panServo;
		
        SkeletonPoint _hipCenter;
        ColorImagePoint _targetPoint;
		KinectSensorChooser _sensorChooser = new KinectSensorChooser();

		Int32Rect _imageSize;
        WriteableBitmap _colorBitmap;
        int _stride;
        byte[] _colorData;
        Skeleton[] _allSkeletons;

		BitmapSource _lastColorFrame;
		int _frameCounter; 
		readonly string _path = Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.Desktop), @"Demos\images\");

		public MainWindow()
		{
			InitializeComponent();

        }

        void sensorChooser_KinectChanged(object sender, KinectChangedEventArgs e)
        {
            KinectSensor oldSensor = e.OldSensor;
            KinectSensor newSensor = e.NewSensor;

            if (oldSensor != null)
            {
                StopKinect(oldSensor); 
            }

            newSensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(newSensor_AllFramesReady);
            newSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30); 
            newSensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            newSensor.SkeletonStream.Enable();

            _stride = newSensor.ColorStream.FrameWidth * 4; // R G B empty
            _imageSize = new Int32Rect(0, 0, newSensor.ColorStream.FrameWidth, newSensor.ColorStream.FrameHeight);
            _colorData = new byte[newSensor.ColorStream.FramePixelDataLength]; 
            _colorBitmap = new WriteableBitmap(newSensor.ColorStream.FrameWidth, 
                newSensor.ColorStream.FrameHeight, 
                96, 96, PixelFormats.Bgr32, null);

            _allSkeletons = new Skeleton[MaxSkeletons];

            try
            {
                newSensor.Start();
                uxColorImage.Source = _colorBitmap;     
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message); 
            }
        }

        void newSensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            SetColorImage(e);
            GetSkeleton(e); 
        }

        private void GetSkeleton(AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame frame = e.OpenSkeletonFrame())
            {
                if (frame == null)
                {
                    ResetGun(); 
                    return;
                }

                frame.CopySkeletonDataTo(_allSkeletons);

                Skeleton first = (from sk in _allSkeletons
                                 where sk.TrackingState == SkeletonTrackingState.Tracked
                                      select sk).FirstOrDefault();  

                if (first == null)
	            {
                    uxStatus.Text = "";
                    ResetGun(); 
                    return;
	            }

                if (first.Joints[JointType.HipCenter].TrackingState == JointTrackingState.NotTracked)
                {
                    uxStatus.Text = "";
                    ResetGun(); 
                    return;
                }

                _hipCenter = first.Joints[JointType.HipCenter].Position;

                //Set Ellipse
                _targetPoint = _sensorChooser.Kinect.MapSkeletonPointToColor(_hipCenter,
                    _sensorChooser.Kinect.ColorStream.Format);

                SetTargetPosition(_targetPoint);

                //Set Angle
                // 640 is horizontal width, 320 is center
                // subtract 320 will give me a -1 to 1 value when divided by 320
                // kinect as 57 degree horizontal view port, 28.5 is center 
                // this now will give me -28.5 to 28.5 which is 57 degrees total
                //double panAngle = ((targetPoint.X - 320) / 320) * 28.5;

                var joint = first.Joints[JointType.HipCenter].ScaleTo(640, 480);
                var panAngle = ((joint.Position.X - 320) / 320) * 28.5;

                SetAngle(panAngle); 

                //Update Distance label
                uxStatus.Text = string.Format("Distance = {0}", _hipCenter.Z.ToString("N"));

                if (_hipCenter.Z < 1.5)
                {
                    //shoot
                    uxStatus.Text = "Say Hello to my little friend!";

                    Fire(); 
                }
                else
                {
                    ResetGun();
                }
            }
        }

        private void Fire()
        {
            ThreadPool.QueueUserWorkItem(SaveImage);

            //fire the gun
            SetRelay(true);
        }

        private void ResetGun()
        {

            //turn off gun
            SetRelay(false);            
        }

        private void SetTargetPosition(ColorImagePoint targetPoint)
        {
            Canvas.SetLeft(target, targetPoint.X - target.Width / 2);
            Canvas.SetTop(target, targetPoint.Y - target.Height / 2);
        }

        private void SetColorImage(AllFramesReadyEventArgs e)
        {
            using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
            {
                if (colorFrame == null)
                {
                    return;
                }

                colorFrame.CopyPixelDataTo(_colorData);

                _colorBitmap.WritePixels(_imageSize, _colorData, _stride, 0);
                _lastColorFrame = _colorBitmap;
            }

        }

        private void StopKinect(KinectSensor oldSensor)
        {
            if (oldSensor != null)
            {
                if (oldSensor.IsRunning)
                {

                    oldSensor.Stop(); 
                }
            }
        }

		private void Window_Loaded(object sender, RoutedEventArgs e)
		{
            InitializePhidgets();
            Thread.Sleep(new TimeSpan(hours: 0, minutes: 0, seconds: 3));

            InitServoSettings();

            uxStatus.Text = "System Ready";

            _sensorChooser.KinectChanged += new EventHandler<KinectChangedEventArgs>(sensorChooser_KinectChanged);
            _sensorChooser.Start(); 

		}

		private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
		{
            ResetGun(); 
		}

		private void InitializePhidgets()
		{
			_relayController = new InterfaceKit();
			_relayController.open();

			_servoController = new AdvancedServo();
			_servoController.open();
		}


		private void SaveImage(object state)
		{
			// every 5 frames we'll capture an image
			if (_frameCounter <= 5)
			{
				_frameCounter++;
				return;
			}

			_frameCounter = 0;

			if (!Directory.Exists(_path))
				Directory.CreateDirectory(_path);

			if (_lastColorFrame != null)
			{
				Dispatcher.BeginInvoke(new ThreadStart(() => _lastColorFrame.Save(_path + DateTime.Now.Ticks + ".png", ImageFormat.Png)));
			}
		}

		private void SetRelay(bool enable)
		{
            //fire or stop firing the gun
            _relayController.outputs[0] = enable;
		}

		#region phidget servo
		private void InitServoSettings()
		{
			_panServo = _servoController.servos[0];
			_panServo.Type = ServoServo.ServoType.HITEC_HS5245MG;

			PanMaxPosition = 45;
			PanMinPosition = -45;
			_panServo.Acceleration = 5000;
			
			_panServo.Engaged = true;

			_panServo.SpeedRamping = false;

			// centering pan
			SetAngle((PanMinPosition + PanMaxPosition) / 2.0);
		}

		public double PanMinPosition
		{
			get { return PanRawToAngleValue(_panServo.PositionMin); }
			set { _panServo.PositionMin = PanAngleToRawValue(value); }
		}

		public double PanMaxPosition
		{
			get { return PanRawToAngleValue(_panServo.PositionMax); }
			set { _panServo.PositionMax = PanAngleToRawValue(value); }
		}

		private void SetAngle(double angle)
		{
            //Debug.WriteLine("angle:" + angle); 

			try
			{
				angle = PanAngleToRawValue(angle);

				if (_panServo.PositionMax < angle)
					angle = _panServo.PositionMax;
				else if (_panServo.PositionMin > angle)
					angle = _panServo.PositionMin;

				_panServo.Position = angle;
			}
			catch (Exception ex0)
			{
				// if this hits, something is physically wrong
				Debug.WriteLine(ex0.ToString());
			}
		}

		// these magic numbers are based on the pan-tilt I used.
		// The servo used was salvaged from a past project, the wp7 controlled t-shirt cannon
		// http://channel9.msdn.com/coding4fun/articles/Coding4Fun-Cannon--Project-Overview
		// GDP985A-BM Pan System
		// http://www.servocity.com/html/gdp985a-bm_pan_system.html
		// A different pan system would work and without a doubt what 
		// i used is overkill but it is what I had on hand that didn't need modification.
		private static double PanAngleToRawValue(double value)
		{
			return (value / 4.5) + 57.5;
		}

		private static double PanRawToAngleValue(double value)
		{
			return (value - 57.5) * 4.5;
		}
		#endregion

	}
}
